%WriteDynamicsFile_Cpp

%
%This script is called by DeriveDynamicsDP.m
%

%FUNCTION: 
%   This function writes the dynamics file for double pendulum equation

%Optimize the code and format for C++ execution:
TmpFile1 = 'TmpFileDDth.cpp';
TmpFile2 = 'TmpFileDDphi.cpp';
ccode(Soln.DDth,'file',TmpFile1);
ccode(Soln.DDphi,'file',TmpFile2);

%format the results to  be more useful for this purpose:
[CODE_1, Declarations_1, Variables_1] = ParseTmpFile(TmpFile1,'DDth');
[CODE_2, Declarations_2, Variables_2] = ParseTmpFile(TmpFile2,'DDphi');
CODE = [CODE_1;CODE_2];
Declarations = [Declarations_1; Declarations_2];

%Open the file
fid = fopen('dynamics.cpp','w');

%Write the file
fprintf(fid, 'void dynamics( double* U, double* X, double* P_dynamics, double* dX)    \n');
fprintf(fid, '{                                                     \n');
fprintf(fid, '                                                      \n');

%This is just a standard block of text to alert readers that it is computer
%generated code...
fprintf(fid, '////////////////////////////////////////////////////////// \n');
fprintf(fid, '// DO NOT EDIT THIS FILE								// \n');
fprintf(fid, '//														// \n');
fprintf(fid, '// It has been automatically generated by:				// \n');
fprintf(fid, '//     WriteDynamicsFile.cpp							// \n');
fprintf(fid, '//														// \n');
fprintf(fid, '// The data and names for the class variables come from // \n');
fprintf(fid, '// the file Set_Parameters.m and comments can be found  // \n');
fprintf(fid, '// there.												// \n');
fprintf(fid, '//														// \n');
fprintf(fid, '// Written by Matthew Kelly								// \n');
fprintf(fid, '// matthew.kelly2@gmail.com								// \n');
fprintf(fid, '////////////////////////////////////////////////////////// \n');
fprintf(fid, '                                                         \n');
fprintf(fid, '                                                         \n');
fprintf(fid, '// Declare variables:                                     \n');
fprintf(fid, ' double th, phi, Dth, Dphi;    //State variables          \n');
fprintf(fid, ' double M1, M2, F1_x, F1_y, F2_x, F2_y;   //Input variables       \n');
fprintf(fid, '                                                         \n');

%Now, print a block of automatically generated definitions:
fprintf(fid, ' // Matlab generated local variables                     \n');
for i=1:length(Declarations)
   fprintf(fid, [Declarations{i} '  \n']); 
end

fprintf(fid, '                                                     \n');
fprintf(fid, '// Extract the parameters from the parameter file       \n');
fprintf(fid, '#include "GetParameters_dynamics.h"                   \n');
fprintf(fid, '                                                     \n');
fprintf(fid, '// Store the state variables                         \n');
fprintf(fid, 'th     = X[0];                                     \n');
fprintf(fid, 'phi    = X[1];                                     \n');
fprintf(fid, 'Dth    = X[2];                                     \n');
fprintf(fid, 'Dphi   = X[3];                                     \n');
fprintf(fid, '                                                     \n');
fprintf(fid, '// Store the input variables                     \n');
fprintf(fid, 'M1   = U[0];                                       \n');
fprintf(fid, 'M2   = U[1];                                       \n');
fprintf(fid, 'F1_x = U[2];                                       \n');
fprintf(fid, 'F1_y = U[3];                                       \n');
fprintf(fid, 'F2_x = U[4];                                       \n');
fprintf(fid, 'F2_y = U[5];                                       \n');
fprintf(fid, '                                                     \n');

%Now, print a block of automatically generated equations:
fprintf(fid, ' // Automatically generated equations:                     \n');
for i=1:length(CODE)
   fprintf(fid, [CODE{i} '  \n']); 
end


fprintf(fid, '                                                     \n');
fprintf(fid, '// Pack up the state derivative                    \n');
fprintf(fid, 'dX[0] = Dth;                                     \n');
fprintf(fid, 'dX[1] = Dphi;                                     \n');
fprintf(fid, 'dX[2] = DDth;                                     \n');
fprintf(fid, 'dX[3] = DDphi;                                     \n');
fprintf(fid, '                                                     \n');
fprintf(fid,'}                                                  \n');


%Close the file!
fclose(fid);
